#!/usr/bin/ruby

# Copyright (C) 2019 Open Source Robotics Foundation
#
# Licensed under the Apache License, Version 2.0 (the "License");
# you may not use this file except in compliance with the License.
# You may obtain a copy of the License at
#
#     http://www.apache.org/licenses/LICENSE-2.0
#
# Unless required by applicable law or agreed to in writing, software
# distributed under the License is distributed on an "AS IS" BASIS,
# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
# See the License for the specific language governing permissions and
# limitations under the License.

# We use 'dl' for Ruby <= 1.9.x and 'fiddle' for Ruby >= 2.0.x
if RUBY_VERSION.split('.')[0] < '2'
  require 'dl'
  require 'dl/import'
  include DL
else
  require 'fiddle'
  require 'fiddle/import'
  include Fiddle
end

require 'optparse'
require 'erb'
require 'pathname'

# Constants.
LIBRARY_NAME = '@library_location@'
LIBRARY_VERSION = '@PROJECT_VERSION_FULL@'

COMMON_OPTIONS =
               "  -h [--help]                Print this help message.\n"\
               "                                                    \n"        +
               "  --force-version <VERSION>  Use a specific library version.\n"\
               "                                                    \n"        +
               '  --versions                 Show the available versions.'

COMMANDS = { 'sim' =>
  "Run and manage Gazebo simulations.                                              \n"\
  "                                                                                \n"\
  "  gz sim [options] [file]                                                       \n"\
  "                                                                                \n"\
  "                                                                                \n"\
  "Available Options:                                                              \n"\
  "  -g                           Run only the GUI.                                \n"\
  "\n"\
  "  --initial-sim-time [arg]     Initial simulation time, in seconds.             \n"\
  "\n"\
  "  --iterations [arg]           Number of iterations to execute.                 \n"\
  "\n"\
  "  --levels                     Use the level system. The default is false,      \n"\
  "                               which loads all models. It's always true         \n"\
  "                               with --network-role.                             \n"\
  "\n"\
  "  --network-role [arg]         Participant role used in a distributed           \n"\
  "                               simulation environment. Role is one of           \n"\
  "                               [primary, secondary]. It implies --levels.       \n"\
  "\n"\
  "  --network-secondaries [arg]  Number of secondary participants expected        \n"\
  "                               to join a distributed simulation                 \n"\
  "                               environment. (Primary only).                     \n"\
  "\n"\
  "  --record                     Use logging system to record states and          \n"\
  "                               console messages to the default location,        \n"\
  "                               in ~/.gz/sim/log.                       \n"\
  "\n"\
  "  --record-path [arg]          Implicitly invokes --record, and specifies       \n"\
  "                               custom path to put recorded files. Argument      \n"\
  "                               is path to record states and console             \n"\
  "                               messages. Specifying this argument will          \n"\
  "                               enable console logging to a console.log          \n"\
  "                               file in the specified path.                      \n"\
  "\n"\
  "  --record-resources           Implicitly invokes --record, and records         \n"\
  "                               meshes and material files, in addition to        \n"\
  "                               states and console messages.                     \n"\
  "\n"\
  "  --record-topic [arg]         Specify the name of an additional topic to       \n"\
  "                               record. Implicitly invokes --record.             \n"\
  "                               Zero or more topics can be specified by          \n"\
  "                               using multiple --record-topic options.           \n"\
  "                               Regular expressions can be used, which           \n"\
  "                               likely requires quotes. A default set of         \n"\
  "                               topics are also recorded, which support          \n"\
  "                               simulation state playback. Enable debug          \n"\
  "                               console output with the -v 4 option              \n"\
  "                               and look for 'Recording default topic' in        \n"\
  "                               order to determine the default set of            \n"\
  "                               topics.                                          \n"\
  "                               Examples:                                        \n"\
  "                                 1. Record all topics.                          \n"\
  "                                     --record-topic \".*\"                      \n"\
  "                                 2. Record only the /stats topic.               \n"\
  "                                     --record-topic /stats                      \n"\
  "                                 3. Record the /stats and /clock topics.        \n"\
  "                                     --record-topic /stats \                    \n"\
  "                                     --record-topic /clock                      \n"\
  "\n"\
  "  --record-period [arg]        Specify the time period (seconds) between        \n"\
  "                               state recording.                                 \n"\
  "\n"\
  "  --log-overwrite              When recording, overwrite existing files.        \n"\
  "                               Only valid if recording is enabled.              \n"\
  "\n"\
  "  --log-compress               When recording, compress final log files.        \n"\
  "                               Only valid if recording is enabled.              \n"\
  "\n"\
  "  --seed [arg]                 Pass a custom seed value to the random           \n"\
  "                               number generator.                                \n"\
  "\n"\
  "  --playback [arg]             Use logging system to play back states.          \n"\
  "                               Argument is path to recorded states.             \n"\
  "\n"\
  "  --headless-rendering         Run rendering in headless mode                   \n"\
  "\n"\
  "  -r                           Run simulation on start.                         \n"\
  "\n"\
  "  -s                           Run only the server (headless mode). This        \n"\
  "                               overrides -g, if it is also present.             \n"\
  "\n"\
  "  -v [ --verbose ] [arg]       Adjust the level of console output (0~4).        \n"\
  "                               The default verbosity is 1, use -v without       \n"\
  "                               arguments for level 3.                           \n"\
  "\n"\
  "  --gui-config [arg]           Gazebo GUI configuration file to load.           \n"\
  "                               If no config is given, the configuration in      \n"\
  "                               the SDF file is used. And if that's not          \n"\
  "                               provided, the default installed config is        \n"\
  "                               used.                                            \n"\
  "\n"\
  "  --physics-engine [arg]       Gazebo Physics engine plugin to load.            \n"\
  "                               Gazebo will use DART by default.                 \n"\
  "                               (gz-physics-dartsim-plugin)                \n"\
  "                               Make sure custom plugins are in                  \n"\
  "                               GZ_SIM_PHYSICS_ENGINE_PATH.                      \n"\
  "\n"\
  "  --render-engine [arg]        Gazebo Rendering engine plugin to load for       \n"\
  "                               both the server and the GUI. Gazebo will use     \n"\
  "                               OGRE2 by default. (ogre2)                        \n"\
  "                               Make sure custom plugins are in                  \n"\
  "                               GZ_SIM_RENDER_ENGINE_PATH.                       \n"\
  "\n"\
  "  --render-engine-api-backend [arg]                                             \n"\
  "                               API to use for both the Server & GUI.            \n"\
  "                               Possible values for ogre2:                       \n"\
  "                                 - opengl (default)                             \n"\
  "                                 - vulkan (beta)                                \n"\
  "                                 - metal (Apple only, default for Apple)        \n"\
  "                               Note: If using Vulkan in the GUI and gz-gui      \n"\
  "                               was built against Qt < 5.15.2, it may be very    \n"\
  "                               slow.                                            \n"\
  "\n"\
  "  --render-engine-gui [arg]    Gazebo Rendering engine plugin to load for       \n"\
  "                               the GUI. Gazebo will use OGRE2 by default.       \n"\
  "                               (ogre2)                                          \n"\
  "                               Make sure custom plugins are in                  \n"\
  "                               GZ_SIM_RENDER_ENGINE_PATH.                       \n"\
  "\n"\
  "  --render-engine-gui-api-backend [arg]                                         \n"\
  "                               Same as --render-engine-api-backend but only     \n"\
  "                               for the GUI.                                     \n"\
  "\n"\
  "  --render-engine-server [arg] Gazebo Rendering engine plugin to load for       \n"\
  "                               the server. Gazebo will use OGRE2 by default.    \n"\
  "                               (ogre2)                                          \n"\
  "                               Make sure custom plugins are in                  \n"\
  "                               GZ_SIM_RENDER_ENGINE_PATH.                       \n"\
  "\n"\
  "  --render-engine-server-api-backend [arg]                                      \n"\
  "                               Same as --render-engine-api-backend but only     \n"\
  "                               for the server.                                  \n"\
  "\n"\
  "  --version                    Print Gazebo version information.                \n"\
  "\n"\
  "  -z [arg]                     Update rate in Hertz.                            \n"\
  "\n"+
  COMMON_OPTIONS + "\n\n" +
  "Environment variables:                                                          \n"\
  "  GZ_SIM_RESOURCE_PATH         Colon separated paths used to locate             \n"\
  " resources such as worlds and models.                                         \n\n"\
  "  GZ_SIM_SYSTEM_PLUGIN_PATH    Colon separated paths used to                    \n"\
  " locate system plugins.                                                       \n\n"\
  "  GZ_SIM_SERVER_CONFIG_PATH    Path to server configuration file.             \n\n"\
  "  GZ_GUI_PLUGIN_PATH           Colon separated paths used to locate GUI         \n"\
  " plugins.                                                                       \n"\
  "  GZ_GUI_RESOURCE_PATH    Colon separated paths used to locate GUI              \n"\
  " resources such as configuration files.                                       \n\n"
}

#
# Class for the Gazebo command line tools.
#
class Cmd

  def killProcess(pid, name, timeout)
    Process.kill("-INT", pid)

    sleepSecs = 0.001
    iterations = (timeout / sleepSecs).to_i
    i = 0
    killedPid = 0
    while killedPid != pid && i < iterations
      begin
        killedPid = Process.waitpid(pid, Process::WNOHANG)
      rescue
        # The process has exited, so return.
        return
      end

      break if killedPid == pid

      sleep sleepSecs
      i = i + 1
    end

    if killedPid != pid
      puts "Escalating to SIGKILL on [#{name}]"
      Process.kill("-KILL", pid)
    end
  end

  #
  # Return a structure describing the options.
  #
  def parse(args)
    options = {
      'file' => '',
      'gui' => 0,
      'hz' => -1,
      'initial_sim_time' => 0,
      'iterations' => 0,
      'levels' => 0,
      'network_role' => '',
      'network_secondaries' => 0,
      'record' => 0,
      'record-path' => '',
      'record-resources' => 0,
      'record-topics' => [],
      'record-period' => -1,
      'log-overwrite' => 0,
      'log-compress' => 0,
      'playback' => '',
      'run' => 0,
      'server' => 0,
      'verbose' => '1',
      'gui_config' => '',
      'physics_engine' => '',
      'render_engine_gui' => '',
      'render_engine_gui_api_backend' => '',
      'render_engine_server' => '',
      'render_engine_server_api_backend' => '',
      'headless-rendering' => 0,
      'wait_gui' => 1,
      'seed' => 0
    }

    usage = COMMANDS[args[0]]

    opt_parser = OptionParser.new do |opts|
      opts.banner = usage

      opts.on('-h', '--help') do
        puts usage
        exit
      end
      opts.on('--iterations [arg]', Integer,
              'Number of iterations to execute') do |i|
        options['iterations'] = i
      end
      opts.on('--network-role [arg]', String) do |role|
        options['network_role'] = role
      end
      opts.on('--network-secondaries [arg]', Integer) do |i|
        options['network_secondaries'] = i
      end
      opts.on('-z [arg]', Float, 'Update rate in Hertz') do |h|
        options['hz'] = h
      end
      opts.on('--initial-sim-time [arg]', Float,
              'Initial simulation time, in seconds.') do |t|
        options['initial_sim_time'] = t
      end
      opts.on('-r') do
        options['run'] = 1
      end
      opts.on('-g') do
        options['gui'] = 1
        # Runing the Gui only, don't show world loading menu
        options['wait_gui'] = 0
      end
      opts.on('-s') do
        options['server'] = 1
        # Runing the server only, don't wait for starting world from Gui
        options['wait_gui'] = 0
      end
      opts.on('--levels') do
        options['levels'] = 1
      end
      opts.on('--record') do
        options['record'] = 1
      end
      opts.on('--record-path [arg]', String) do |r|
        options['record-path'] = r
      end
      opts.on('--record-resources') do
        options['record-resources'] = 1
      end
      opts.on('--record-topic [arg]', String) do |t|
        options['record-topics'].append(t)
      end
      opts.on('--record-period [arg]', Float) do |d|
        options['record-period'] = d
      end
      opts.on('--log-overwrite') do
        options['log-overwrite'] = 1
      end
      opts.on('--log-compress') do
        options['log-compress'] = 1
      end
      opts.on('--playback [arg]', String) do |p|
        options['playback'] = p
      end
      opts.on('-v [verbose]', '--verbose [verbose]', String) do |v|
        options['verbose'] = v || '3'
      end
      opts.on('--gui-config [arg]', String) do |c|
        options['gui_config'] = c
      end
      opts.on('--physics-engine [arg]', String) do |e|
        options['physics_engine'] = e
      end
      opts.on('--headless-rendering') do
        options['headless-rendering'] = 1
      end
      opts.on('--render-engine-gui [arg]', String) do |g|
        options['render_engine_gui'] = g
      end
      opts.on('--render-engine-gui-api-backend [arg]', String) do |a|
        options['render_engine_gui_api_backend'] = a
      end
      opts.on('--render-engine-server [arg]', String) do |k|
        options['render_engine_server'] = k
      end
      opts.on('--render-engine-server-api-backend [arg]', String) do |a|
        options['render_engine_server_api_backend'] = a
      end
      opts.on('--render-engine [arg]', String) do |f|
        options['render_engine_gui'] = f
        options['render_engine_server'] = f
      end
      opts.on('--render-engine-api-backend [arg]', String) do |a|
        options['render_engine_gui_api_backend'] = a
        options['render_engine_server_api_backend'] = a
      end
      opts.on('--version') do
        options['version'] = '1'
      end
      opts.on('--seed [arg]', Integer) do |i|
        options['seed'] = i
      end

    end # opt_parser do

    opt_parser.parse!(args)

    # SDF file as positional argument
    filename = args.pop
    if filename and filename != 'sim' and filename != 'gazebo'
      options['file'] = filename
    end

    options['command'] = args[0]

    options
  end # parse()

  def execute(args)
    options = parse(args)

    library_name_path = Pathname.new(LIBRARY_NAME)
    if library_name_path.absolute?
      # If the first character is a slash, we'll assume that we've been given an
      # absolute path to the library. This is only used during test mode.
      plugin = LIBRARY_NAME
    else
      # We're assuming that the library path is relative to the current
      # location of this script.
      plugin = File.expand_path(File.join(File.dirname(__FILE__), LIBRARY_NAME))
    end
    conf_version = LIBRARY_VERSION

    begin
      Importer.dlload plugin
    rescue DLError => e
      puts "Library error for [#{plugin}]: #{e.to_s}"
      if plugin.end_with? ".dylib"
        puts "
If this script was executed with /usr/bin/ruby, this error may be caused by
macOS System Integrity Protection. One workaround is to use a different
version of ruby, for example:
    brew install ruby
and add the following line to your shell profile:
    export PATH=/usr/local/opt/ruby/bin:$PATH
If you are using a colcon workspace, please ensure that the setup script
has properly set the DYLD_LIBRARY_PATH environment variables."
      end
      exit(-1)
    end

    # Read the library version.
    Importer.extern 'char *gzSimVersion()'
    begin
      plugin_version = Importer.gzSimVersion.to_s
    rescue DLError
      puts "Library error: Problem running 'gzSimVersion()' from #{plugin}."
      exit(-1)
    end

    # Sanity check: Verify that the version of the yaml file matches the version
    # of the library that we are using.
    unless plugin_version.eql? conf_version
      puts "Error: Version mismatch. Your configuration file version is
            [#{conf_version}] but #{plugin} version is [#{plugin_version}]."
      exit(-1)
    end

    usage = COMMANDS[args[0]]

    begin

      # Import the findFuelResource function
      Importer.extern 'const char *findFuelResource(const char *)'

      if options.key?('version')
        Importer.extern 'char *simVersionHeader()'
        puts Importer.simVersionHeader.to_s
        exit
      end

      # Global configurations
      if options.key?('verbose')
        Importer.extern 'void cmdVerbosity(const char *)'
        Importer.cmdVerbosity(options['verbose'])
      end

      parsed = ''
      if options['file'] != ''
        # Check if the passed in file exists.
        if File.exist?(options['file'])
          path = options['file']
        # If not, then first check the GZ_SIM_RESOURCE_PATH environment
        # variable, then the configuration path from the launch library.
        else
          resourcePathEnv = ENV['GZ_SIM_RESOURCE_PATH']

          if resourcePathEnv.nil?
            resourcePathEnv = ENV['IGN_GAZEBO_RESOURCE_PATH']

            if !resourcePathEnv.nil?
              puts "
Using deprecated environment variable [IGN_GAZEBO_RESOURCE_PATH].
Please use [GZ_SIM_RESOURCE_PATH] instead."
            end
          end

          if !resourcePathEnv.nil?
            resourcePaths = resourcePathEnv.split(':')
            for resourcePath in resourcePaths
              filePath = File.join(resourcePath, options['file'])
              if File.exist?(filePath)
                path = filePath
                break
              end
            end
          end

          if path.nil?
            Importer.extern 'char *worldInstallDir()'
            path = File.join(Importer.worldInstallDir().to_s, options['file'])
            if !File.exist?(path)
              path = Importer.findFuelResource(options['file']).to_s
              options['file'] = path
              if path == ""
                puts "Unable to find or download file " + options['file']
                exit(-1)
              end
            end
          end
        end

        # ERB parse the file, and then run the result
        parsed = ERB.new(File.read(path)).result()
      end

      # Import the runServer function
      Importer.extern 'int runServer(const char *, int, int, float, double, int,
                               const char *, int, int, const char *,
                               int, int, int, const char *, const char *,
                               const char *, const char *, const char *,
                               const char *, const char *,
                               const char *, int, int, float, int)'

      # Import the runGui function
      Importer.extern 'int runGui(const char *, const char *, int,
                                  const char *, const char *)'

      # If playback is specified, and the user has not specified a
      # custom gui config, set the gui config to load the playback
      # gui config
      if (options['playback'] != '' and options['gui_config'] == '')
        options['gui_config'] = "_playback_"
      end

      # Neither the -s nor -g options were used, so run both the server
      # and gui.
      if options['server'] == 0 && options['gui'] == 0

        if plugin.end_with? ".dylib"
          puts "On macOS `gz sim` currently only works with either the -s argument
or the -g argument, you cannot run both server and gui in one terminal.
See https://github.com/gazebosim/gz-sim/issues/44 for more info."
          exit(-1)
        end

        if plugin.end_with? ".dll"
          puts "`ign gazebo` currently only works with the -s argument on Windows.
See https://github.com/gazebosim/gz-sim/issues/168 for more info."
          exit(-1)
        end

        serverPid = Process.fork do
          ENV['RMT_PORT'] = '1500'
          Process.setpgid(0, 0)
          Process.setproctitle('gz sim server')
          Importer.runServer(parsed,
            options['iterations'], options['run'], options['hz'],
            options['initial_sim_time'], options['levels'],
            options['network_role'], options['network_secondaries'],
            options['record'], options['record-path'],
            options['record-resources'], options['log-overwrite'],
            options['log-compress'], options['playback'],
            options['physics_engine'],
            options['render_engine_server'],
            options['render_engine_server_api_backend'],
            options['render_engine_gui'],
            options['render_engine_gui_api_backend'],
            options['file'], options['record-topics'].join(':'),
            options['wait_gui'],
            options['headless-rendering'], options['record-period'],
            options['seed'])
        end

        guiPid = Process.fork do
          ENV['RMT_PORT'] = '1501'
          Process.setpgid(0, 0)
          Process.setproctitle('gz sim gui')
          Importer.runGui(options['gui_config'], options['file'],
                          options['wait_gui'], options['render_engine_gui'],
                          options['render_engine_gui_api_backend'])
        end

        Signal.trap("INT") {
          self.killProcess(guiPid, "Gazebo Sim GUI", 5.0)
          self.killProcess(serverPid, "Gazebo Sim Server", 5.0)
          return 1
        }

        # Wait for a child process to end
        pid, status = Process.wait2

        if pid == serverPid
          self.killProcess(guiPid, "Gazebo Sim GUI", 5.0)
        else
          self.killProcess(serverPid, "Gazebo Sim Server", 5.0)
        end

      # If the -s option was specified, then run only the server
      elsif options['server'] == 1
        ENV['RMT_PORT'] = '1500'
        Importer.runServer(parsed, options['iterations'], options['run'],
            options['hz'], options['initial_sim_time'], options['levels'],
            options['network_role'], options['network_secondaries'],
            options['record'], options['record-path'],
            options['record-resources'], options['log-overwrite'],
            options['log-compress'], options['playback'],
            options['physics_engine'],
            options['render_engine_server'],
            options['render_engine_server_api_backend'],
            options['render_engine_gui'],
            options['render_engine_gui_api_backend'],
            options['file'], options['record-topics'].join(':'),
            options['wait_gui'], options['headless-rendering'],
            options['record-period'], options['seed'])
            # Otherwise run the gui
      else options['gui']
        if plugin.end_with? ".dll"
          puts "`gz sim` currently only works with the -s argument on Windows.
See https://github.com/gazebosim/gz-sim/issues/168 for more info."
          exit(-1)
        end

        ENV['RMT_PORT'] = '1501'
        Importer.runGui(options['gui_config'], options['file'],
                        options['wait_gui'], options['render_engine_gui'],
                        options['render_engine_gui_api_backend'])
      end
    end
  # execute
  end
# class
end
